#include <Arduino.h>
// #include <string.h>
#include "fpga_reciver.h"
#include "udp.h"
ModbusRTU mb;
ModbusIP mb_tcp;  //ModbusIP object

const char startMarker = 'A';
const char endMarker = 'B';
const int maxDataSize = 8;
int H_data = 10;
int L_data = 10;
int cos_data = 0 ;
int sin_data = 0 ;
int received_length_Data;
int received_cos_Data;
int received_sin_Data;

byte receivedBytes[maxDataSize];

int dataIndex = 0;
bool newData = false;

void Serial_callback() {
  while (Serial1.available() > 0) 
  {
    char receivedChar = Serial1.read();

    if (receivedChar == startMarker) {
      dataIndex = 0;
      newData = true;
    } 
    else if (newData)
    {
      if (dataIndex < maxDataSize) 
      {
        receivedBytes[dataIndex] = receivedChar;
        dataIndex++;
      } 
      else 
      {
        // 解析完成
        if (dataIndex == maxDataSize && receivedChar == endMarker) 
        {
          received_length_Data = (receivedBytes[0] << 24) |(receivedBytes[1] << 16) | (receivedBytes[2] << 8) | receivedBytes[3];
          received_cos_Data = (receivedBytes[4] << 10) |(receivedBytes[5] << 2) ;
          received_sin_Data  = (receivedBytes[5] << 10) |(receivedBytes[6] << 2) ;
          H_data = (receivedBytes[0] << 8) | receivedBytes[1];
          L_data = (receivedBytes[2] << 8) | receivedBytes[3];
          cos_data = (receivedBytes[4] << 10) |(receivedBytes[5] << 2) ;
          sin_data = (receivedBytes[6] << 10) |(receivedBytes[7] << 2) ;

          mb.Hreg(REGN10, H_data);
          mb.Hreg(REGN11, L_data);
          mb.Hreg(REGN12, cos_data);
          mb.Hreg(REGN13, sin_data);

          received_length_Data = received_length_Data & 0x7FFFFFFF; // Mask out the sign bit
          // Serial.println(receivedBytes[0]);
          if (receivedBytes[0] & 0x80) {
            received_length_Data = -received_length_Data; // Apply the sign if the sign bit is set
          }

          received_cos_Data = received_cos_Data & 0x7FFF; // Mask out the sign bit
          // Serial.println(receivedBytes[0]);
          if (receivedBytes[4] & 0x80) {
            received_cos_Data = -received_cos_Data; // Apply the sign if the sign bit is set
          }
          received_cos_Data = received_cos_Data >> 2 ;

          received_sin_Data = received_sin_Data & 0x7FFF; // Mask out the sign bit
          // Serial.println(receivedBytes[0]);
          if (receivedBytes[6] & 0x80) {
            received_sin_Data = -received_sin_Data; // Apply the sign if the sign bit is set
          }
          received_sin_Data = received_sin_Data >> 2 ;          
        }
        newData = false;
      }
    }
  }
}



void fpga_reciver_init() {
  // HardwareSerial(Serial1,921600,);
  Serial1.begin(921600,SERIAL_8N1,12,13);
  // Serial1.begin(921600);
  Serial1.onReceive(Serial_callback);//设置回调函数
  // #if defined(ESP32) || defined(ESP8266)
  mb.begin(&Serial0);
  // #else
  //   mb.begin(&Serial);
  //   //mb.begin(&Serial, RXTX_PIN);  //or use RX/TX direction control pin (if required)
  //   mb.setBaudrate(9600);
  // #endif
    mb.slave(SLAVE_ID);
    mb.addHreg(REGN10);
    mb.addHreg(REGN11);
    mb.addHreg(REGN12);
    mb.addHreg(REGN13);
    mb.addHreg(REGN14);
    mb.Hreg(REGN10, H_data);
    mb.Hreg(REGN11, L_data);
    // mb.Hreg(REGN12, 0);
    // mb.Hreg(REGN13, 0);
    // mb.Hreg(REGN14, 0);
  mb_tcp.server(TCP_PORT);		//Start Modbus IP
  // Add SENSOR_IREG register - Use addIreg() for analog Inputs
  mb_tcp.addHreg(REGN10);
  mb_tcp.addHreg(REGN11);
  mb_tcp.addHreg(REGN12);
  mb_tcp.addHreg(REGN13);
  mb_tcp.addHreg(REGN14);  
  mb_tcp.setTransactionId(1);



}